#-------------------------------------------------------------------------------
# 模块名称:    计算速度障碍模型 Velocity Obstacle Model.实现
# 模块功能:    实现Velocity Obstacle Model模型，采用不同模型
#-------------------------------------------------------------------------------
# 工作原理：   通过等距离投影（Equirectangular_Projection），基于勾股定理（Pythagorean_Theorem）
# 适用范围：   因为将球坐标转化为直角坐标并运用勾股定理，所以只能在两点相聚较近时使用，纬度越高，使用范围越窄。
#              资料表明，仅仅适用于近距离（中纬地区40KM以内）
# 输入：       本船信息ship1，包括：本船中心点坐标，最大航速，
#              他船信息ship2，包括：他船中心点坐标，实时航速，真北角，长宽->椭圆领域长短轴a, b
# 输出：       速度障碍模型六元组等信息
# 模块作者:    Youan
# 创建时间:    13-05-2021
# 版权所有：   (c) Youan 2021
#-------------------------------------------------------------------------------

import SimMath
import math

def calcVelocity_Obstacle_Model(ship1={}, ship2={}, method = 0):
    """
    # 目的：计算速度障碍模型Velocity_Obstacle_Model
    # 参数：
        本船ship1 : 字典格式，{'lon': 111, 'lat':31, 'head': 70, 'vMax': 30}
        来船ship2 : 字典格式，{'lon': 111, 'lat':31, 'head': 70, 'speed': 10}
    # 返回值：列表[P0, P1, P2, P3, P4, P5, P6] ，
              其中：P0为来船中心点，P1为本船中心点...
    """

    def computeP3P4():
        # P3P4：通过“求一条过P2的直线L，且此直线与PO-P1（的延长线）垂直，L与以P1为圆心、V1max为半径的圆的交点，即P3，P4。”
        # 即：过P2的直线的方位角为"直线P0-P1的方位角加90度"，注意将坐标系换算成 X-Y米勒坐标计算夹角
        # 约定：P3在向量P1P0的左侧，即向量P2P3为向量P1P0的逆时针方向
        #       P4在右侧，即P2P4为向量P1P0顺时针方向
        vector = angle + 90 # 逆时针旋转90度
        beta, res = SimMath.calcLineCircleIntersect(P2xy, vector, ship1Circle)

        if beta == None:
            P3xy = P4xy = P3 = P4 = (None, None)
            thetaP = thetaN = None
        else:
            thetaP = beta[0]
            thetaN = beta[1]
            P3 = SimMath.calcTargetPosition(P1[0], P1[1], thetaP, R)
            P3xy = SimMath.convertionLonlatToXY(P1, P3)
            P4 = SimMath.calcTargetPosition(P1[0], P1[1], thetaN, R)
            P4xy = SimMath.convertionLonlatToXY(P1, P4)
            # 测试从向量P1P0方向到P2P3，期望值为假（逆时针），若为真值则交换P3，P4：
            if SimMath.calcAngleVectors((P1xy, P0xy), (P2xy, P3xy), 2):
                P3, P4 = P4, P3
                P3xy, P4xy = P4xy, P3xy
                thetaP, thetaN = thetaN, thetaP
        return P3, P4, P3xy, P4xy, thetaP, thetaN
        pass

    def computeP5P6Simple():
        # P5P6：求一条过P2的直线L，且此直线与P1-T1/P1-T2平行，L与以P1为圆心、
        #       V1max为半径的圆的交点，分别为P5，P6。约定：P5处于P1P0左侧，P6处于右侧。
        # 采用为近似算法：

        def computeP56Simple(dCircle, isP5 ):
            P56xy = P56 = (None, None)
            Theta = None
            if (dCircle >= distP1P0) or (distP1P0 == 0):
                return P56, P56xy, Theta
            pass
            if isP5:
                theta56 = angleP1P0 - math.degrees(math.asin(dCircle / distP1P0))
            else:
                theta56 = angleP1P0 + math.degrees(math.asin(dCircle / distP1P0))
            beta, res = SimMath.calcLineCircleIntersect(P2xy, theta56, ship1Circle)
            if beta == None:
                return P56, P56xy, Theta

            point = res[0]  # 预选第1点（P5/P6点）
            Theta = beta[0]
            # P5在左侧，测试从向量P1P0方向到P2P5，期望值为假（逆时针），若为真值则取另一个值：
            # P6在右侧，测试从向量P2P6方向到P1P0，期望值为假（逆时针），若为真值则取另一个值：
            if isP5:
                if SimMath.calcAngleVectors((P1xy, P0xy), (P2xy, point), 2):
                    Theta = beta[1]
            else:
                if SimMath.calcAngleVectors((P2xy, point), (P1xy, P0xy), 2):
                    Theta = beta[1]
            P56 = SimMath.calcTargetPosition(P1[0], P1[1], Theta, R)
            P56xy = SimMath.convertionLonlatToXY(P1, P56)
            return P56, P56xy, Theta
            pass

        def computeP5():
            P5xy = P5 = (None, None)
            Theta = None
            if (d2 >= distP1P0) or (distP1P0 == 0):
                return P5, P5xy, Theta
            thetaN = angleP1P0 - math.degrees(math.asin(d2 / distP1P0))
            beta, res = SimMath.calcLineCircleIntersect(P2xy, thetaN, ship1Circle)
            if beta == None:
                return P5, P5xy, Theta
            point = res[0]  # 预设P5点，P5P2, P1P0夹角期望为正
            Theta = beta[0]
            # P5在左侧，测试从向量P1P0方向到P2P5，期望值为假（逆时针），若为真值则取另一个值：
            if SimMath.calcAngleVectors((P1xy, P0xy), (P2xy, point), 2):
                Theta = beta[1]
            P5 = SimMath.calcTargetPosition(P1[0], P1[1], Theta, R)
            P5xy = SimMath.convertionLonlatToXY(P1, P5)
            return P5, P5xy, Theta
            pass

        def computeP6():
            P6xy = P6 = (None, None)
            Theta = None
            if (d1 >= distP1P0) or (distP1P0 == 0):
                return P6, P6xy, Theta
            thetaP = angleP1P0 + math.degrees(math.asin(d1 / distP1P0))
            beta, res = SimMath.calcLineCircleIntersect(P2xy, thetaP, ship1Circle)
            if beta == None:
                return P6, P6xy, Theta
            point = res[0]  # 预设P6点，P0P1P6夹角期望为正
            Theta = beta[0]
            # P6在右侧，测试从向量P2P6方向到P1P0，期望值为假（逆时针），若为真值则取另一个值：
            if SimMath.calcAngleVectors((P2xy, point), (P1xy, P0xy), 2):
                Theta = beta[1]
            P6 = SimMath.calcTargetPosition(P1[0], P1[1], Theta, R)
            P6xy = SimMath.convertionLonlatToXY(P1, P6)
            return P6, P6xy, Theta
            pass

        angleP1P0 = SimMath.calcTrueHeading(P1[0], P1[1], P0[0], P0[1])
        #P5, P5xy, theta5 = computeP5()
        #P6, P6xy, theta6 = computeP6()
        P5, P5xy, theta5 = computeP56Simple(d2, True)
        P6, P6xy, theta6 = computeP56Simple(d1, False)
        return P5, P6, P5xy, P6xy, theta5, theta6
        pass

    def computeP5P6EllipseDomain():
        # P5P6：求一条过P2的直线L，且此直线与P1-T1/P1-T2平行，L与以P1为圆心、
        #       V1max为半径的圆的交点，分别为P5，P6。约定：P5处于P1P0左侧，P6处于右侧。
        # 采用为近似算法：

        def computeP56Ellipse(theta, isP5):
            P56xy = P56 = (None, None)
            Theta = None
            beta, res = SimMath.calcLineCircleIntersect(P2xy, theta, ship1Circle)
            if beta == None:
                return P5, P5xy, Theta
            point = res[0] # 预设P5点，P5P2P0夹角期望为正
            Theta = beta[0]
            # P5在左侧，测试从向量P1P0方向到P2P5，期望值为假（逆时针），若为真值则取另一个值：
            # P6在右侧，测试从向量P2P6方向到P1P0，期望值为假（逆时针），若为真值则取另一个值：
            if isP5:
                if SimMath.calcAngleVectors((P1xy, P0xy), (P2xy, point), 2):
                    Theta = beta[1]
            else:
                if SimMath.calcAngleVectors((P2xy, point), (P1xy, P0xy), 2):
                    Theta = beta[1]
            P56 = SimMath.calcTargetPosition(P1[0], P1[1], Theta, R)
            P56xy = SimMath.convertionLonlatToXY(P1, P56)
            return P56, P56xy, Theta
            pass

        def computeP5():
            P5xy = P5 = (None, None)
            Theta = None
            beta, res = SimMath.calcLineCircleIntersect(P2xy, thetaP, ship1Circle)
            if beta == None:
                return P5, P5xy, Theta
            point = res[0] # 预设P5点，P5P2P0夹角期望为正
            Theta = beta[0]
            # P5在左侧，测试从向量P1P0方向到P2P5，期望值为假（逆时针），若为真值则取另一个值：
            if SimMath.calcAngleVectors((P1xy, P0xy), (P2xy, point), 2):
                Theta = beta[1]
            P5 = SimMath.calcTargetPosition(P1[0], P1[1], Theta, R)
            P5xy = SimMath.convertionLonlatToXY(P1, P5)
            return P5, P5xy, Theta
            pass

        def computeP6():
            P6xy = P6 = (None, None)
            Theta = None
            beta, res = SimMath.calcLineCircleIntersect(P2xy, thetaN, ship1Circle)
            if beta == None:
                return P6, P6xy, Theta
            point = res[0] # 预设P6点，P0P2P6夹角期望为正
            Theta = beta[0]
            # P6在右侧，测试从向量P2P6方向到P1P0，期望值为假（逆时针），若为真值则取另一个值：
            if SimMath.calcAngleVectors((P2xy, point), (P1xy, P0xy), 2):
                Theta = beta[1]
            P6 = SimMath.calcTargetPosition(P1[0], P1[1], Theta, R)
            P6xy = SimMath.convertionLonlatToXY(P1, P6)
            return P6, P6xy, Theta
            pass

        P5xy = P6xy = P5 = P6 = (None, None)
        theta5 = theta6 = None
        T1T2 = SimMath.calcTangentPoint4GeneralEllipse(P1xy, (a, b, xe, ye, head2))
        if T1T2 == []:
            print('P1xy, (a, b, xe, ye, head2)=', P1xy, (a, b, xe, ye, head2))
            return P5, P6, P5xy, P6xy, theta5, theta6
        [thetaP, thetaN, pointTP, pointTN] = T1T2
        theta = math.degrees(math.atan2(xe - P1xy[0], ye-P1xy[1]))
        thetaP = theta + thetaP
        thetaN = theta + thetaN
        #P5, P5xy, theta5 = computeP5()
        #P6, P6xy, theta6 = computeP6()
        P5, P5xy, theta5 = computeP56Ellipse(thetaP, True)
        P6, P6xy, theta6 = computeP56Ellipse(thetaN, False)
        return P5, P6, P5xy, P6xy, theta5, theta6
        pass

    def computeP7P8():
        # P7P8：求一条过P1的直线L，且此直线与ship1的方位角垂直
        theta7 = head1 + 90
        theta8 = head1 - 90
        P7 = SimMath.calcTargetPosition(P1[0], P1[1], theta7, R)
        P7xy = SimMath.convertionLonlatToXY(P1, P7)
        P8 = SimMath.calcTargetPosition(P1[0], P1[1], theta8, R)
        P8xy = SimMath.convertionLonlatToXY(P1, P8)
        return P7, P8, P7xy, P8xy, theta7, theta8

    # 读取参数
    lon1 = ship1.get('lon', 0)
    lat1 = ship1.get('lat', 0)
    head1= ship1.get('head', 0)
    lon2 = ship2.get('lon', 0)
    lat2 = ship2.get('lat', 0)
    V2   = ship2.get('speed', 0)
    head2= ship2.get('head', 0)

    V1max= ship1.get('vMax', 20)

    # P1：本船中心点坐标
    P1 = (lon1, lat1)
    P1xy = (0, 0)  # P1-为xy坐标原点

    # P0：来船椭圆领域中心点坐标
    P0 = (lon2, lat2)
    P0xy = SimMath.convertionLonlatToXY(P1, P0)

    # 若采用椭圆领域算法，读取相关数据，并计算椭圆中心点和方位角
    leng = ship2.get('length', 60)
    a = 2 * leng
    b = 1.5 * leng
    beta = 21.80140948635181 + head2
    dist = 0.5385164807134504 * leng
    sa = math.sin(math.radians(head2))
    ca = math.cos(math.radians(head2))
    xe = P0xy[0] + dist * math.sin(math.radians(beta))
    ye = P0xy[1] + dist * math.cos(math.radians(beta))
    Pexy = (xe, ye)

    # 基础参数，计算P1P0距离
    distP1P0 = SimMath.calcDistanceHaversine(P1[0], P1[1], P0[0], P0[1])
    status = 0 # 默认，需要计算VO
    # 检查初始参数是否符合忽略条件
    if (method == 0) and (distP1P0 > 4298):
        status == -1    # 安全，可忽略Ignore
    if (method == 0) and (distP1P0 < 103):
        status == 1    # 全风险，无需计算
    if (method == 1) and (distP1P0 > 70 * leng):
        status == -1    # 安全，可忽略Ignore
    if (method == 1) and (distP1P0 < 2.4 * leng):
        status == 1    # 全风险，无需计算
    if (status != 0):
        return [P0xy, P1xy], {'status': status}

    # P2：通过“以P1为原点，绘制V2，所得到的终点为P2”
    # 即以P1为出发点，沿着head2行走V2距离所得到的坐标点
    P2 = SimMath.calcTargetPosition(P1[0], P1[1], head2, V2)
    P2xy = SimMath.convertionLonlatToXY(P1, P2)


    # 基础中间辅助量
    angle = SimMath.calcTrueHeading(P0[0], P0[1], P1[0], P1[1])   #基本参考矢量P0P1
    R = V1max
    ship1Circle = (P1xy[0], P1xy[1], R)   # 本船参考圆

    # P3P4：通过“求一条过P2的直线L，且此直线与PO-P1（的延长线）垂直，L与以P1为圆心、V1max为半径的圆的交点，即P3，P4。”
    # 即：过P2的直线的方位角为"直线P0-P1的方位角加90度"，注意将坐标系换算成 X-Y米勒坐标计算夹角
    # 约定：P3在向量P1P0的左侧，即向量P2P3为向量P1P0的逆时针方向
    #       P4在右侧，即P2P4为向量P1P0顺时针方向
    P3, P4, P3xy, P4xy, thetaP, thetaN = computeP3P4()
    heads = [thetaP, thetaN]

    # P5P6：求一条过P2的直线L，且此直线与P1-T1/P1-T2平行，L与以P1为圆心、
    #       V1max为半径的圆的交点，分别为P5，P6。约定：P5处于P1P0左侧，P6处于右侧。
    theta = math.radians(head2 - head1)
    d1 = 100 + math.sin(theta)
    d2 = 50 - 50 * math.cos(theta)
    if method == 0:
        # 简单近似算法
        P5, P6, P5xy, P6xy, theta5, theta6 = computeP5P6Simple()
    if method == 1:
        # 椭圆领域算法
        P5, P6, P5xy, P6xy, theta5, theta6 = computeP5P6EllipseDomain()
    heads += [theta5, theta6]
    P7, P8, P7xy, P8xy, theta7, theta8 = computeP7P8()
    heads += [theta7, theta8]

    Pxy = [P0xy, P1xy, P2xy, P3xy, P4xy, P5xy, P6xy, P7xy, P8xy]
    for i in range(len(Pxy)):
        Pxy[i] = list(Pxy[i])
        if Pxy[i][0] == None:
            status = 1
            break
    res = {'status': status}
    res['head'] = heads.copy()
    # 近似算法，返回两个参考圆信息
    res['d'] = [d1, d2]
    # 椭圆领域算法，返回椭圆信息 a, b, xe, ye
    #Pe = convertionXYToLonlat(P1, Pexy)
    res['ell'] = [a, b, Pexy]
    res['radius'] = R
    # return Pxy, res
    return {'VOPxy': Pxy, 'status': status, 'head': head1, 'speed': ship1.get('speed', 0), 'radius': R}, res

